#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <EIGEN/SVD>
#include <pcl/common/common.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

#include <sophus/se2.hpp>

class Icp2DWithLine
{
public:
    Icp2DWithLine():src_cloud_(new pcl::PointCloud<pcl::PointXY>)
    {
        laser_sub_ = nh_.subscribe<sensor_msgs::LaserScan>("/scan", 1, std::bind(&Icp2DWithLine::laserCallBack, this,std::placeholders::_1));
    }


private:
    void laserCallBack(const sensor_msgs::LaserScanConstPtr& scan)
    { 
        if(first_scan_)
        {
            if(src_cloud_ == nullptr)
                src_cloud_.reset(new pcl::PointCloud<pcl::PointXY>);
            //1.将scan转换点云
            src_cloud_->points.clear();
            for(size_t i = 0;i<scan->ranges.size();++i)
            {
                if(scan->ranges[i]<0.1 || scan->ranges[i]>18)
                    continue;
                pcl::PointXY pt;
                double real_angle = scan->angle_min + i * scan->angle_increment;
                pt.x = scan->ranges[i] * cos(real_angle);
                pt.y = scan->ranges[i] * sin(real_angle);
                src_cloud_->points.emplace_back(pt);
            }
            if(src_cloud_->points.size()<30)
            {
                ROS_WARN("Not enough points in src cloud");
                return;
            }
            kd_tree_.setInputCloud(src_cloud_);
            first_scan_ = false;
            return;
        }

        //开始进行最近邻匹配
        const size_t iter_count = 10;
        const size_t min_match_num = 30;
        double cost = 0.0,last_cost = 0.0;
        const double max_dis = 0.02;

        for(size_t iter = 0;iter != iter_count;++iter)
        {
            Sophus::SE2d T;
            size_t match_num = 0;
            Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
            Eigen::Vector3d b = Eigen::Vector3d::Zero();
            for(size_t i = 0;i< scan->ranges.size();++i)
            {
                if(scan->ranges[i]<0.1 || scan->ranges[i]>18)
                    continue;
                //
                double real_angle = scan->angle_min + i*scan->angle_increment;
                double real_x = scan->ranges[i]*cos(real_angle);
                double real_y = scan->ranges[i]*sin(real_angle);
                Eigen::Vector2d p(real_x,real_y);
                Eigen::Vector2d pw = T*p;

                //寻找最近的五个点
                std::vector<int> idx;
                std::vector<float> dis;
                kd_tree_.nearestKSearch(pcl::PointXY{float(pw.x()),float(pw.y())},5,idx,dis);
                if(dis.size()>=5 && dis[4]<=max_dis)
                {
                    ++match_num;
                }
                //对这5个点进行直线拟合
                Eigen::Vector3d line_param = Eigen::Vector3d::Zero();
                std::vector<pcl::PointXY> points;
                for(size_t id = 0;id<idx.size();++id)
                {
                    points.emplace_back(src_cloud_->points[idx[id]]);
                }
                lineFitting(points,line_param);
                Eigen::Vector3d J;
                double yaw = T.so2().log();
                J<< line_param[0],line_param[1],-line_param[0]*scan->ranges[i]*sin(real_angle+yaw)+line_param[1]*scan->ranges[i]*cos(real_angle+yaw);

                H+= J*J.transpose();
                // b+= J*e;   //e为点到直线的距离
            }
        }


    }


    void lineFitting(const std::vector<pcl::PointXY>& point,Eigen::Vector3d& line_param)
    {
        if(point.size()<2)
        {
            std::cout<<"line fitting error: point size < 2"<<std::endl;
            return;
        }

        Eigen::Vector3d center = Eigen::Vector3d::Zero();
        for(int i = 0;i<point.size();i++)
        {
            center += Eigen::Vector3d{point[i].x,point[i].y,0.0};
        }
        center /= point.size();

        Eigen::MatrixXd A(point.size(),3);
        for(int i = 0;i<point.size();i++)
        {
            A.row(i) = Eigen::Vector3d{point[i].x-center(0),point[i].y-center(1),0.0};
        }
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(A,Eigen::ComputeThinU | Eigen::ComputeThinV);
        //todo:奇异值最小的为直线

    }


private:
    ros::NodeHandle nh_;
    ros::Subscriber laser_sub_;

    bool first_scan_ = true;
    pcl::PointCloud<pcl::PointXY>::Ptr src_cloud_;
    sensor_msgs::LaserScanConstPtr src_scan_;

    pcl::KdTreeFLANN<pcl::PointXY> kd_tree_;

};